#ifndef _SENYUNDRIVER_H
#define _SENYUNDRIVER_H

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>

#include "opencv2/core/cuda.hpp"
#include "opencv2/cudawarping.hpp"

#include <thread>
#include <iostream>
#include <string>
#include "module_base/module_base.h"
#include "sensor_msgs/Image.h"
#include "undistort.h"
#include "senyun_driver/gmsl_camera_handler.h"

// All vpi headers are under directory vpi/
#include <vpi/OpenCVInterop.hpp>

#include <vpi/Image.h>
#include <vpi/Status.h>
#include <vpi/Stream.h>
#include <vpi/Context.h>

// Algorithms we'll need
#include <vpi/algo/BoxFilter.h>
#include <vpi/algo/ConvertImageFormat.h>
#include <vpi/algo/Remap.h>
#include <vpi/LensDistortionModels.h>
#include <vpi/algo/Rescale.h>


#define CHECK_VPI_STATUS(STMT)                                \
    do                                                        \
    {                                                         \
        VPIStatus status = (STMT);                            \
        if (status != VPI_SUCCESS)                            \
        {                                                     \
            char buffer[VPI_MAX_STATUS_MESSAGE_LENGTH];       \
            vpiGetLastStatusMessage(buffer, sizeof(buffer));  \
            std::ostringstream ss;                            \
            ss << vpiStatusGetName(status) << ": " << buffer; \
            throw std::runtime_error(ss.str());               \
        }                                                     \
    } while (0);

using namespace v4l_cuda;

namespace senyun_driver
{

    class SenyunDriver : public module_base::ModuleBase
    {
    public:
        SenyunDriver(){};
        ~SenyunDriver(){};
        virtual void onInit();

    private:
        void loop();
        void get_a_frame();
        void undistort_image(cv::Mat &origin_im, cv::Mat &undis_im);

    private:
        char *image_yuyv_cuda_ = NULL;
        unsigned char *image_rgb_cuda_ = NULL;
        unsigned char *image_undis_cuda_ = NULL;
        float param_[9];
        bool extern_use_undis_;
        bool extern_use_compress_;
        bool extern_use_crop_;
        std::thread thread_;
        cv::Mat intrinsic_matrix;
        cv::Mat distort_;
        std::string device_name_;
        bool use_undis_;
        bool use_compress_;
        uint8_t *undis_input_gpu_;
        uint8_t *undis_output_gpu_;
        std::shared_ptr<GmslCameraHandler> camera_handler;
        cv::Mat input_image = cv::Mat::zeros(1080, PIXL_WIDTH, CV_8UC2);
        cv::Mat input_image_new = cv::Mat::zeros(1080, PIXL_WIDTH_N, CV_8UC2);
        cv::Mat output_image = cv::Mat::zeros(1080, PIXL_WIDTH_N, CV_8UC4);;
                cv::Mat output_image_resize = cv::Mat::zeros(1080/2, PIXL_WIDTH_N/2, CV_8UC4);;

        VPIImage image_vpi_UYVY, image_vpi_NV12ER_in, image_vpi_NV12ER_in_resize, image_vpi_NV12ER_out, image_vpi_NV12ER_out_resize, image_vpi_BGRA8, image_vpi_BGRA8_resize, image_vpi_BGR8, image_vpi_BGR8_resize;
        VPIStream stream;
        VPIConvertImageFormatParams cvtParams;
        // VPIContext ctx = 0;
    };
}

#endif